#!/usr/bin/env python 

import rospy
from std_msgs.msg import Float64

flaga = 0

class Loop:
    def __init__(self):
        rospy.on_shutdown(self.cleanup)
        # 初始化舵机
        self.joint1 = rospy.Publisher('/tilt_controller1/command', Float64, queue_size=10)
        self.joint2 = rospy.Publisher('/tilt_controller2/command', Float64, queue_size=10)
        self.joint3 = rospy.Publisher('/tilt_controller3/command', Float64, queue_size=10)
        self.joint4 = rospy.Publisher('/tilt_controller4/command', Float64, queue_size=10)
        self.joint5 = rospy.Publisher('/tilt_controller5/command', Float64, queue_size=10)
        self.pos = Float64()

        while not rospy.is_shutdown():
           if flaga == 0:
               # 设置每颗舵机的目标弧度
                self.pos = 2.6
               # 发布舵机运动指令
                self.joint1.publish(self.pos)
                self.pos = 0.6
                self.joint2.publish(self.pos)
                self.pos = 2.6
                self.joint3.publish(self.pos)
                self.pos = 3.2
                self.joint4.publish(self.pos)
                self.pos = 3
                self.joint5.publish(self.pos)
                flag = 1
                rospy.sleep(3)

    
        # rospy.sleep(3)
        #    if flaga == 0:
        #         self.pos = 3
        #         self.joint5.publish(self.pos)
        #         rospy.sleep(3)
        #         flag=1
        # while not rospy.is_shutdown():
        #     self.pos = 3
        #     self.joint5.publish(self.pos)
        #     rospy.sleep(3)
        #     break




    def cleanup(self):
        rospy.loginfo('Shutting down robot arm ...')

if __name__ == '__main__':
    rospy.init_node('arm')
    try:
        Loop()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass